#include "motor_cmd.h"
#include "mc_param.h"
#include "stdio.h"
#include "main.h"
#include "shell.h"
#include "mc_control.h"

extern Motor_t motor;
extern PidHandle_t spd_pid;
extern PidHandle_t iq_pid;
extern PidHandle_t id_pid;
void motor_control_cmd(uint8_t cmd, int16_t data)
{
    if (cmd < 5) // cmd about timer and timer's ccr value, debug
    {

        if(motor.mc_foc_param.control_mod != control_null)
        {
            TIM1->CCR1 = 0;
            TIM1->CCR2 = 0;
            TIM1->CCR3 = 0;
            motor.mc_foc_param.control_mod = control_null;
        }
        switch (cmd)
        {
        case 1: // channel_1    ccr1
            TIM1->CCR1 = data;
            printf("TIM1 CCR1 IS: %d", TIM1->CCR1);
            break;
        case 2: // channel_1    ccr2
            TIM1->CCR2 = data;
            printf("TIM1 CCR2 IS: %d", TIM1->CCR2);
            break;
        case 3: // channel_1    ccr3
            TIM1->CCR3 = data;
            printf("TIM1 CCR3 IS: %d", TIM1->CCR3);
            break;
        case 4: // channel_1    ccr4
            TIM1->CCR4 = data;
            printf("TIM1 CCR4 IS: %d", TIM1->CCR4);
            break;
        default:
            break;
        }
    }
    else if (cmd == 6) // cmd about adc control regsistor
    {
        if (data > 10) // only have ten control menthods
        {
            return;
        }
        switch (data)
        {
        case 1: // triger edge down
            motor.adc_content.adc_trigger_edge = 0;
            break;
        case 2: // triger edge up
            motor.adc_content.adc_trigger_edge = 1;
            break;
        case 3: //
            break;
        default:
            break;
        }
    }
    else if(cmd == 7)//printf out FOC parameter for debug
    {
        uint8_t sector, sector_act;
        int16_t theta, theta_act;
        uint16_t sa, sb, sc;
        int16_t ia, ib, ic;
        int32_t angle_raw, eangle, angle_act;
        for(uint16_t i = 0; i < data; i++)
        {
            sector = motor.encoder_content.sector;
            theta = motor.encoder_content.virtual_theta;
            sa = motor.mc_foc_param.sa;
            sb = motor.mc_foc_param.sb;
            sc = motor.mc_foc_param.sc;
            ia = motor.mc_foc_param.ia_norm;
            ib = motor.mc_foc_param.ib_norm;
            ic = motor.mc_foc_param.ic_norm;
            angle_raw = motor.encoder_content.encoder_count_pre[0];
            eangle = (((angle_raw * MOTOR_POLESPAIR)%16384) - 8192)<<2;
            angle_act = motor.encoder_content.e_angle_act;
            sector_act = motor.encoder_content.sector_act;
            theta_act = motor.encoder_content.e_angle_act;
            printf("%d, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d\r\n", \
            sector, sector_act, theta, theta_act, sa, sb, sc, ia, ib, ic, angle_raw, eangle, angle_act);
        }
    }
    else if(cmd == 8)//change motor control method
    {
        switch (data)
        {
        case 0:
            if(motor.mc_foc_param.control_mod != control_null)
            {
                TIM1->CCR1 = 0;
                TIM1->CCR2 = 0;
                TIM1->CCR3 = 0;
                motor.mc_foc_param.control_mod = control_null;
            }
            break;
        case 1:
            motor.mc_foc_param.control_mod = torque_mode;
            break;
        case 2:
            motor.mc_foc_param.control_mod = speed_mode;
            break;
        default:
            break;
        }
    }
    else if (cmd == 9)//torque control, input torque reference, form 0 ~ 32767
    {
        motor.mc_foc_param.idq_ref.q = data;
    }
    else if (cmd == 10)//spd reference input form 0 ~ 32767, is 1024 times of normal speed with unit rad/s
    {
        motor.mc_foc_param.spd_ref = data;
    }
    else if(cmd == 11)//foc current id iq output 
    {
        MC_Dq_t iqd, uqd;
        for(uint16_t i = 0; i < data; i++)
        {
            uqd.d = motor.mc_foc_param.udq.d;
            uqd.q = motor.mc_foc_param.udq.q;
            iqd.d = motor.mc_foc_param.idq.d;
            iqd.q = motor.mc_foc_param.idq.q;
            printf(" %d, %d, %d, %d\r\n", \
            iqd.d, iqd.q, uqd.d, uqd.q);
        }
    }
    else if(cmd == 12)//motor speed output, spd is normal speed with unit rad/s, spd_norm = spd * 1024
    {
        int32_t spd, spd_norm;
        for(uint16_t i = 0; i < data; i++)
        {
            spd = motor.encoder_content.speed_rad;
            spd_norm = motor.encoder_content.speed_rad_nom;
            printf(" %d, %d, %d\r\n", \
            spd, spd_norm, motor.encoder_content.encoder_count_pre[0]);
        }
    }
    else if(cmd == 13)
    {
        spd_pid.kp_gain = data;
        // spd_pid.integral_term = 0;
        printf("spd kp is: %d\r\n", spd_pid.kp_gain);
    }
    else if(cmd == 14)
    {
        spd_pid.ki_gain = data;
        // spd_pid.integral_term = 0;
        printf("spd ki is: %d\r\n", spd_pid.ki_gain);
    }
    else if(cmd == 15)
    {
        id_pid.kp_gain = data;
        // id_pid.integral_term = 0;
        printf("id kp is: %d\r\n", id_pid.kp_gain);
    }
    else if(cmd == 16)
    {
        id_pid.ki_gain = data;
        // id_pid.integral_term = 0;
        printf("id ki is: %d\r\n", id_pid.ki_gain);
    }
    else if(cmd == 17)
    {
        iq_pid.kp_gain = data;
        // iq_pid.integral_term = 0;
        printf("iq kp is: %d\r\n", iq_pid.kp_gain);
    }
    else if(cmd == 18)
    {
        iq_pid.ki_gain = data;
        // iq_pid.integral_term = 0;
        printf("iq ki is: %d\r\n", iq_pid.ki_gain);
    }
    else if(cmd == 19)//spd pid output
    {
        iq_pid.ki_gain = data;
        // iq_pid.integral_term = 0;
        printf("iq ki is: %d\r\n", iq_pid.ki_gain);
    }

}

SHELL_EXPORT_CMD(cmd, motor_control_cmd, uint8_cmd_int16_data);
